#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#include "stm32f10x.h"

#include "public_lib.h"
#include "serial.h"
#include "adxl_345.h"
#include "pos_task.h"

#define FILTER_LEN (5)

typedef struct
{
    char enable;
    unsigned char index;
    short pos_x[FILTER_LEN];
    short pos_y[FILTER_LEN];
    short pos_z[FILTER_LEN];
}s_adxl345_task_info;

static s_adxl345_task_info info = {0};

void start_adxl345_pos_task(void *args)
{
    ADXL345_Init();
    info.enable = 1;
}

int adxl345_pos_task_run(void *args)
{
    if(info.enable)
    {
        char index = info.index&0x7F;
        ADXL345_RD_XYZ(&info.pos_x[index],&info.pos_y[index],&info.pos_z[index]);
        index ++;        
        if(GET_BIT(info.index, 7) || (index>=5))
        {        
            info.index = 0x80;
        }
        else
        {
            info.index = index;
        }
    }
    
    return 0;
}

int adxl345_pos_task_manage(int argc, char **argv, void *args)
{
    int err_code = -1;

    if(argc >= 2)
    {
       
        if(strcasecmp(argv[0], "adxl345")==0)
        {
            if(strcasecmp(argv[1], "enable") == 0)
            {
                info.enable = 1;   
            }            
            else if(strcasecmp(argv[1], "disable") == 0)
            {                
                info.enable = 0;   
                info.index = 0;
            } 
            else if(strcasecmp(argv[1], "show") == 0)
            {                
                short x=0,y=0,z=0;
                get_adxl345_pos(&x, &y, &z);
                serial_writeline("pos : x[%d], y[%d], z[%d]", x,y,z);
            }     
        }        
        else{/* not for me, do nothing */}
    }
    else{/* not for me, do nothing */}

    return err_code;
}

void get_adxl345_pos(short *x, short *y, short *z)
{
    int len = 0,x_sum=0,y_sum=0,z_sum=0;
    
    if(GET_BIT(info.index, 7))
    {
        len = 5;
    }
    else
    {
        len = info.index;
    }
    
    for(int i=0;i<len;i++)
    {
        x_sum += info.pos_x[i];
        y_sum += info.pos_y[i];
        z_sum += info.pos_z[i];        
    }
    
    *x = x_sum / FILTER_LEN;
    *y = y_sum / FILTER_LEN;
    *z = z_sum / FILTER_LEN;
}
